(* Content-type: application/vnd.wolfram.mathematica *)

(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)

(* CreatedBy='Mathematica 11.0' *)

(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[       158,          7]
NotebookDataLength[     10578,        312]
NotebookOptionsPosition[      9771,        281]
NotebookOutlinePosition[     10104,        296]
CellTagsIndexPosition[     10061,        293]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Notebook[{
Cell[BoxData[
 RowBox[{
  RowBox[{"Get", "[", "\"\<RobotManipulator`\>\"", "]"}], ";"}]], "Input",
 CellChangeTimes->{{3.683470647473692*^9, 3.683470660476201*^9}, {
   3.6835535297170467`*^9, 3.6835535380907717`*^9}, {3.683673538296907*^9, 
   3.683673542489224*^9}, {3.683814560579185*^9, 3.6838145616417217`*^9}, 
   3.683815816419771*^9, {3.700217836674705*^9, 3.700217840035198*^9}, {
   3.701128449342113*^9, 3.701128455877973*^9}, {3.7011294001339617`*^9, 
   3.7011294004189262`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"urdfFile", " ", "=", " ", 
   RowBox[{"FileNameJoin", "[", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"NotebookDirectory", "[", "]"}], ",", 
      "\"\<atlas_simple_contact_noback.urdf\>\""}], "}"}], "]"}]}], 
  ";"}]], "Input",
 CellChangeTimes->{
  3.683816071771426*^9, {3.689468760491665*^9, 3.689468765229948*^9}, 
   3.698981581322528*^9, {3.701392215488534*^9, 3.701392224247035*^9}, 
   3.701396989669018*^9}],

Cell[BoxData[
 RowBox[{"args", " ", "=", " ", 
  RowBox[{"<|", 
   RowBox[{
    RowBox[{"\"\<Mass\>\"", "\[Rule]", "3.700000"}], ",", 
    RowBox[{"\"\<Inertia\>\"", "\[Rule]", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0.010267", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0.010267", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0.006660"}], "}"}]}], "}"}]}], ",", 
    RowBox[{"\"\<gst0\>\"", "\[Rule]", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"1", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "1", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "1", ",", "0.089159"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}]}], ",", 
    RowBox[{"\"\<TwistPairs\>\"", "\[Rule]", 
     RowBox[{"{", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "1"}], 
         "}"}], ",", "x$1$1"}], "}"}], "}"}]}], ",", 
    RowBox[{"\"\<ChainIndices\>\"", "\[Rule]", "1"}]}], "|>"}]}]], "Input",
 CellChangeTimes->{{3.702827773993786*^9, 3.702827782097781*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"\[LeftAssociation]", 
  RowBox[{
   RowBox[{"\"\<Mass\>\"", "\[Rule]", "3.7`"}], ",", 
   RowBox[{"\"\<Inertia\>\"", "\[Rule]", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"0.010267`", ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0.010267`", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0.00666`"}], "}"}]}], "}"}]}], ",", 
   RowBox[{"\"\<gst0\>\"", "\[Rule]", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"1", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "1", ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "1", ",", "0.089159`"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}]}], ",", 
   RowBox[{"\"\<TwistPairs\>\"", "\[Rule]", 
    RowBox[{"{", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "1"}], 
        "}"}], ",", "x$1$1"}], "}"}], "}"}]}], ",", 
   RowBox[{"\"\<ChainIndices\>\"", "\[Rule]", 
    RowBox[{"{", "1", "}"}]}]}], "\[RightAssociation]"}]], "Input",
 CellChangeTimes->{{3.702827819694169*^9, 3.7028278237531023`*^9}}],

Cell[BoxData[
 RowBox[{"\[LeftAssociation]", 
  RowBox[{
   RowBox[{"\<\"Mass\"\>", "\[Rule]", "3.7`"}], ",", 
   RowBox[{"\<\"Inertia\"\>", "\[Rule]", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"0.010267`", ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0.010267`", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0.00666`"}], "}"}]}], "}"}]}], ",", 
   RowBox[{"\<\"gst0\"\>", "\[Rule]", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"1", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "1", ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "1", ",", "0.089159`"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}]}], ",", 
   RowBox[{"\<\"TwistPairs\"\>", "\[Rule]", 
    RowBox[{"{", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "1"}], 
        "}"}], ",", "x$1$1"}], "}"}], "}"}]}], ",", 
   RowBox[{"\<\"ChainIndices\"\>", "\[Rule]", 
    RowBox[{"{", "1", "}"}]}]}], "\[RightAssociation]"}]], "Output",
 CellChangeTimes->{3.702827824523342*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"ComputeBodyJacobians", "[", 
  RowBox[{"args", ",", "6"}], "]"}]], "Input",
 CellChangeTimes->{{3.7028277841728888`*^9, 3.7028277922888517`*^9}}],

Cell[BoxData[
 TemplateBox[{
  "Flatten","normal",
   "\"Nonatomic expression expected at position \
\\!\\(\\*RowBox[{\\\"1\\\"}]\\) in \\!\\(\\*RowBox[{\\\"Flatten\\\", \
\\\"[\\\", \\\"1\\\", \\\"]\\\"}]\\).\"",2,15,3,31763117406799988744,"Local"},
  
  "MessageTemplate"]], "Message", "MSG",
 CellChangeTimes->{3.70282779278928*^9, 3.7028278256181183`*^9}],

Cell[BoxData[
 TemplateBox[{
  "Set","pkspec1",
   "\"The expression \\!\\(\\*RowBox[{\\\"Flatten\\\", \\\"[\\\", \\\"1\\\", \
\\\"]\\\"}]\\) cannot be used as a part specification.\"",2,15,4,
   31763117406799988744,"Local"},
  "MessageTemplate"]], "Message", "MSG",
 CellChangeTimes->{3.70282779278928*^9, 3.702827825625328*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{"{", 
   RowBox[{
    RowBox[{"{", 
     RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], "}"}], 
    ",", 
    RowBox[{"{", 
     RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], "}"}], 
    ",", 
    RowBox[{"{", 
     RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], "}"}], 
    ",", 
    RowBox[{"{", 
     RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], "}"}], 
    ",", 
    RowBox[{"{", 
     RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], "}"}], 
    ",", 
    RowBox[{"{", 
     RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], "}"}]}],
    "}"}], "}"}]], "Output",
 CellChangeTimes->{3.702827792801523*^9, 3.7028278256311293`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"np", "=", 
  RowBox[{"Length", "[", 
   RowBox[{"{", "args", "}"}], "]"}]}]], "Input",
 CellChangeTimes->{{3.70282786229956*^9, 3.70282787012134*^9}}],

Cell[BoxData["1"], "Output",
 CellChangeTimes->{3.702827870804531*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{"argList", "=", 
   RowBox[{"{", "args", "}"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.7028279151549683`*^9, 3.702827919080637*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"Je", "=", 
   RowBox[{"Table", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"twist", "=", 
       RowBox[{"RobotManipulator`Private`GetTwist", "[", 
        RowBox[{"argList", "[", 
         RowBox[{"[", "i", "]"}], "]"}], "]"}]}], ";", "\[IndentingNewLine]", 
      
      RowBox[{"gs0", "=", 
       RowBox[{"RobotManipulator`Private`GetGST0", "[", 
        RowBox[{"argList", "[", 
         RowBox[{"[", "i", "]"}], "]"}], "]"}]}], ";", "\[IndentingNewLine]", 
      
      RowBox[{"(*", 
       RowBox[{
       "initialize", " ", "the", " ", "Jacobian", " ", "with", " ", "fixed", 
        " ", "length", " ", 
        RowBox[{"(", "ndof", ")"}]}], "*)"}], 
      RowBox[{"Jz", "=", 
       RowBox[{"ConstantArray", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"6", ",", 
           RowBox[{"IntegerPart", "@", "nDof"}]}], "}"}]}], "]"}]}], ";", 
      "\[IndentingNewLine]", 
      RowBox[{"If", "[", 
       RowBox[{
        RowBox[{"!", 
         RowBox[{"ExtraUtils`EmptyQ", "[", "twist", "]"}]}], ",", 
        RowBox[{"(*", 
         RowBox[{
         "the", " ", "link", " ", "is", " ", "the", " ", "base", " ", 
          "link"}], "*)"}], 
        RowBox[{"(*", 
         RowBox[{
         "assign", " ", "the", " ", "dependent", " ", "coordinate", " ", 
          "indices"}], "*)"}], 
        RowBox[{
         RowBox[{"curIndices", "=", 
          RowBox[{"GetChainIndices", "[", 
           RowBox[{"argList", "[", 
            RowBox[{"[", "i", "]"}], "]"}], "]"}]}], ";", 
         "\[IndentingNewLine]", 
         RowBox[{"(*", 
          RowBox[{"compute", " ", "spatial", " ", "jacobian"}], "*)"}], 
         RowBox[{
          RowBox[{"Jz", "[", 
           RowBox[{"[", 
            RowBox[{";;", ",", "curIndices"}], "]"}], "]"}], "=", 
          RowBox[{"RobotLinks`BodyJacobian", "[", 
           RowBox[{
            RowBox[{"Sequence", "@@", "twist"}], ",", "gs0"}], "]"}]}], 
         ";"}]}], "]"}], ";", "\[IndentingNewLine]", "Jz"}], ",", 
     RowBox[{"{", 
      RowBox[{"i", ",", "np"}], "}"}]}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.702827875896936*^9, 3.702827907454081*^9}}]
},
WindowSize->{928, 1028},
WindowMargins->{{Automatic, 0}, {0, Automatic}},
FrontEndVersion->"11.0 for Linux x86 (64-bit) (July 28, 2016)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)

(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[558, 20, 492, 8, 35, "Input"],
Cell[1053, 30, 456, 12, 35, "Input"],
Cell[1512, 44, 1308, 33, 72, "Input"],
Cell[CellGroupData[{
Cell[2845, 81, 1301, 33, 83, InheritFromParent],
Cell[4149, 116, 1276, 33, 72, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[5462, 154, 169, 3, 32, "Input"],
Cell[5634, 159, 359, 8, 23, "Message"],
Cell[5996, 169, 330, 7, 23, "Message"],
Cell[6329, 178, 778, 22, 52, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[7144, 205, 174, 4, 32, "Input"],
Cell[7321, 211, 70, 1, 30, "Output"]
}, Open  ]],
Cell[7406, 215, 168, 4, 32, "Input"],
Cell[7577, 221, 2190, 58, 217, "Input"]
}
]
*)

